;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

; BLOCKENV.LISP
; Perceptions and actions for a simulated Blocks World environment.

; All blocks on table with none adjacent.  

(defvar state* NIL)

;;; initial1* A B C D on table
(defparameter initial1*
      (list '(block A xpos 2 ypos 2 width 2 height 2 shade 3) 
	    '(block B xpos 6 ypos 2 width 2 height 2 shade 3) 
	    '(block C xpos 10 ypos 2 width 2 height 2 shade 3) 
	    '(block D xpos 14 ypos 2 width 2 height 2 shade 3) 
	    '(table T1 xpos 0 ypos 0 width 20 height 2 shade 3)
	    '(hand H1 status empty)))

#-RAB
;;; initial2* tower <D-C-B-A|
(defparameter initial2*
  (list '(block A xpos 2 ypos 2 width 2 height 2 shade 3) 
	'(block B xpos 2 ypos 4 width 2 height 2 shade 3) 
	'(block C xpos 2 ypos 6 width 2 height 2 shade 3) 
	'(block D xpos 2 ypos 8 width 2 height 2 shade 3) 
	'(table T1 xpos 0 ypos 0 width 20 height 2 shade 3)
	'(hand H1 status empty)))

#-RAB
;;; initial3* tower <A-B-C-D| on-table E
(defparameter initial3*
  (list '(block A xpos 2 ypos 8 width 2 height 2 shade 3) 
	'(block B xpos 2 ypos 6 width 2 height 2 shade 3) 
	'(block C xpos 2 ypos 4 width 2 height 2 shade 3) 
	'(block D xpos 2 ypos 2 width 2 height 2 shade 3) 
	'(block E xpos 6 ypos 2 width 2 height 2 shade 3) 
	'(table T1 xpos 0 ypos 0 width 20 height 2 shade 3)
	'(hand H1 status empty)))

#-RAB
;;; initial4* tower <A-B-C-D| tower <E-F-G-H| on-table I
(defparameter initial4*
  (list '(block A xpos 2 ypos 8 width 2 height 2 shade 3) 
	'(block B xpos 2 ypos 6 width 2 height 2 shade 3) 
	'(block C xpos 2 ypos 4 width 2 height 2 shade 3) 
	'(block D xpos 2 ypos 2 width 2 height 2 shade 3) 
	'(block E xpos 8 ypos 8 width 2 height 2 shade 3) 
	'(block F xpos 8 ypos 6 width 2 height 2 shade 3) 
	'(block G xpos 8 ypos 4 width 2 height 2 shade 3) 
	'(block H xpos 8 ypos 2 width 2 height 2 shade 3)
	'(block I xpos 12 ypos 2 width 2 height 2 shade 3)
	'(table T1 xpos 0 ypos 0 width 20 height 2 shade 3)
	'(hand H1 status empty)))

(defvar initial*)

;; make initial0* the default
(setf initial* initial1*)

(defun task1 ()
  (setf initial* initial1*))

; Define a function that Icarus will call to initialize the environment.
(defun initialize-world ()
  (setq state* (rcopy initial*))
  nil)

; Define a function that Icarus will call to update the environment.
(defun update-world () nil)

; Define a function that Icarus will call to update the perceptual buffer.
(defun preattend () (copy-list state*))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Envirnonment Actions Code
;;;      (these are the only functions that should be called in primitive skills
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


(defun *grasp-and-raise (block)
  (cond ((and
	  (block? block)
	  (block-clear? block)
	  (hand-empty?)
	  )
	 ;; pickup block from table
	 (*grasp-and-lift block))
	(t (warn "FAILED in attempt to pickup block ~a" block))))

(defun *place-and-ungrasp (block target)
  (let ((placement (compute-placement-on-target block target)))
    (cond (placement
	   (*put-down-at-x-y block
			     (first placement)
			     (second placement)))
	  (t (warn "*place-and-ungrasp failed to place block: ~a on target: ~a"
		   block target)))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; auxilliary code  (helper functions) -- not to be called by skills
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun compute-placement-on-target (block target)
  (cond ((block? target)
	 (compute-placement-on-block block target))
	((table? target)
	 (compute-placement-on-table block target))
	(t (warn "Placement target: ~a is not a block or table" target))))

;; (defun compute-placement-on-block (block target-block)
;;   (cond ((and (block? block)
;; 	      (block-clear? target-block)
;; 	      (holding-block? block)
;; 	      (not (wider? block target-block)))
;; 	 (list (+ (object-left target-block)
;; 		  (floor (- (object-width target-block)
;; 			    (object-width block))
;; 			 2))		; try to center (otherwise round to the left)
;; 	       (object-top target-block)))
;; 	(t (warn "Couldn't find place for block: ~a on target: ~a" block target-block)
;; 	   nil)))

;; new way to compute placement on block: always left align when
;; setting down
(defun compute-placement-on-block (block target)
  (cond ((and (block? block)
	      (block-clear? target)
	      (holding-block? block)
	      (not (wider? block target)))
	 (list (object-left target)
	       (object-top target)))
	(t (warn "Couldn't find place for block: ~a on target: ~a"
		 block target)
	   nil)))

(defun compute-placement-on-table (block table)
  (cond ((and (block? block)
	      (holding-block? block))
	 (let ((x-place (compute-x-place-on-table table (object-width block)))
	       (y-place (object-top table)))
	   (when x-place
	     (list x-place y-place))))
	(t (warn "Couldn't find place on table: ~a wide enought for block: ~a of width: ~a"
		 table block (object-width block))
	   nil)))
  
(defun compute-x-place-on-table (table width)
  (let* ((table-width (object-width table))
	 (blocks-on-table (blocks-on-object table))
	 (blocks-on-table-left-right-pairs
	  (loop for block in blocks-on-table
	     collect
	     (list (object-left block)
		   (object-right block))))
	 (sorted-left-right-pairs
	  (sort blocks-on-table-left-right-pairs #'< :key #'first)))
    (loop for (left1 right1) in (cons '(0 0) sorted-left-right-pairs)
       for (left2 right2) in (append sorted-left-right-pairs (list (list table-width table-width)))
       when (>= (- left2 right1)
		width)
       return right1)))
		 


;; Accessors

(defun object-form (object-name)
  (first (member object-name state* :key #'second)))

(defun object-left (object-name)
  (second (member 'xpos (object-form object-name))))

(defun object-bottom (object-name)
  (second (member 'ypos (object-form object-name))))

(defun object-width (object-name)
  (second (member 'width (object-form object-name))))

(defun object-height (object-name)
  (second (member 'height (object-form object-name))))

(defun object-top (object-name)
  (+ (object-bottom object-name)
     (object-height object-name)))

(defun object-right (object-name)
  (+ (object-left object-name)
     (object-width object-name)))

(defun get-hand-contents (&optional (state state*))
  (second (member 'status (assoc 'hand state))))


;; Predicates
(defun block? (object)
  (eql 'block (first (object-form object))))

(defun table? (object)
  (eql 'table (first (object-form object))))

(defun wider? (obj1 obj2)
  (> (object-width obj1)
     (object-width obj2)))

(defun block-clear? (block)
  (null (blocks-on-object block)))

(defun hand-empty? ()
  (eql 'empty (get-hand-contents)))

(defun holding-block? (block)
  (eql block (get-hand-contents)))



;;; utilities

(defun blocks-on-object (object)	; block or table
  (let ((object-bounds (left-bottom-right-top object)))
    (unless object-bounds
      (warn "No bounds found for object: ~a" object))
    (loop with (left bottom right top) = object-bounds
       for form in state*
       when (and (eql (first form) 'block)
		 (bounds-on-top? (left-bottom-right-top (second form))
				 object-bounds))
	 collect
	 (second form))))

(defun left-bottom-right-top (object)
  (list (object-left object)
	(object-bottom object)
	(object-right object)
	(object-top object)))

(defun bounds-on-top? (bounds-above bounds-below)
  (and bounds-above
       bounds-below
       (= (fourth bounds-below) (second bounds-above))
       (<= (first bounds-below) (first bounds-above))
       (>= (third bounds-below) (third bounds-above))))





; Define an action for grasping a block.
(defun *grasp (block)
  (let* ((object (assoc 'hand state*))
	 (rest (member 'status object)))
;    (cond ((not (null atrace*))
;	   (terpri)(princ "Grasping ")(princ block)))
    (setf (cadr rest) block)))

; Define an action for ungrasping a block.
(defun *ungrasp (block)
  (let* ((object (assoc 'hand state*))
	 (rest (member 'status object)))
;    (cond ((not (null atrace*))
;	   (terpri)(princ "Ungrasping ")(princ block)))
    (setf (cadr rest) 'empty)))

; Define an action for moving a block vertically.
(defun *vertical-move (block ypos)
  (Let* ((object-form (object-form block))
	 (rest (member 'ypos object-form)))
;    (cond ((not (null atrace*))
;	   (terpri)(princ "Moving ")(princ block)
;	   (princ " to vertical position ")(princ ypos)))
    (setf (cadr rest) ypos)))

; Define an action for moving a block horizontally.
(defun *horizontal-move (block xpos)
  (let* ((object-form (object-form block))
	 (rest (member 'xpos object-form)))
;    (cond ((not (null atrace*))
;	   (terpri)(princ "Moving ")(princ block)
;	   (princ " to horizontal position ")(princ xpos)))
    (setf (cadr rest) xpos)))

;; support actions (to allow single action calls in defined primitive skills)

(defun *grasp-and-lift (block &optional (y 100))
  (progn
    ;(cond ((not (null atrace*))
    ;       (format t "~%Grasping and lifting block ~a to y = ~a" block y)))
    (*grasp block)
    (*vertical-move block y)))

;; (x,y) is the placement location for the lower-left corner of the block
(defun *put-down-at-x-y (block x y)
  (progn
    ;(cond ((not (null atrace*))
    ;       (format t "~%Placing block ~a at position x = ~a, y = ~a" 
    ;               block x y)))
    (*horizontal-move block x)
    (*vertical-move block y)  	; original had (+ y h) instead of y
    (*ungrasp block)
    ;(when width
    ;   (*update-free 'FREE (+ x width)))
    ))

